#!/usr/bin/env python
import rospy, cv2, cv_bridge, numpy
from sensor_msgs.msg import Image
from sensor_msgs.msg import LaserScan
from geometry_msgs.msg import Twist

class Obstacle():
    def __init__(self):
        self.LIDAR_ERR = 0.20
        self._cmd_pub = rospy.Publisher('cmd_vel', Twist, queue_size=1)
        
        self.bridge = cv_bridge.CvBridge()
        #cv2.namedWindow("window", 1)
        self.image_sub = rospy.Subscriber('camera/rgb/image_raw',Image, self.image_callback)
        self.twist = Twist()
        self.goForward = False

        self.obstacle()
        self.k=0.01
        self.k2=0.01

    def image_callback(self, msg):
        image = self.bridge.imgmsg_to_cv2(msg,desired_encoding='bgr8')
        hsv = cv2.cvtColor(image, cv2.COLOR_BGR2HSV)

        lower_select = numpy.array([ 167,  100,  90])
        upper_select = numpy.array([ 187, 255, 255])
        mask = cv2.inRange(hsv, lower_select, upper_select)

        h, w, d = image.shape
        # search_top = 3*h/4
        # search_bot = 3*h/4 + 20

        search_top = 0
        search_bot = 0 + 20
        mask[0:search_top, 0:w] = 0
        mask[search_bot:h, 0:w] = 0

        mask_img,contours,hierarchy=cv2.findContours(mask,1,2)
        
        if len(contours) >= 2:
            M = cv2.moments(contours[0])
            M2 = cv2.moments(contours[1])

            cv2.drawContours(image, contours, -1, (0, 255, 0), 10)

            if M['m00'] > 0 and M2['m00'] > 0:
                cx = int(M['m10']/M['m00'])
                cy = int(M['m01']/M['m00'])

                cx2 = int(M2['m10']/M2['m00'])
                cy2 = int(M2['m01']/M2['m00'])

                if abs(cx-cx2) > 50:
                    cxm = (cx+cx2)/2
                    cym = (cy+cy2)/2

                    cv2.circle(image, (cxm, cym), 20, (0,0,255), -1)
                    # BEGIN CONTROL
                    err = cxm - (w/2)
                    if (self.goForward):
                        self.twist.linear.x = 0.08
                        self.twist.angular.z = -float(err) *0.001
                        if self.twist.angular.z > 1:
                            self.twist.linear.x = 0
                            self.twist.angular.z = 0
                        self._cmd_pub.publish(self.twist)
                    # END CONTROL
        cv2.imshow("window", image)
        cv2.waitKey(1)

    def get_scan(self):
        msg = rospy.wait_for_message("scan", LaserScan)
        self.scan_filter = []
        for i in range(170,190):
            if msg.ranges[i] >= self.LIDAR_ERR:
                self.scan_filter.append(msg.ranges[i])

    def obstacle(self):
        while not rospy.is_shutdown():
            self.get_scan()

            if min(self.scan_filter) < 0.7:
                self.twist.linear.x = 0.0
                self.twist.angular.z = 0.0
                self._cmd_pub.publish(self.twist)
                self.goForward = False
                rospy.loginfo('Stop!')

            else:
                self.goForward = True
                rospy.loginfo('distance of the obstacle : %f', min(self.scan_filter))

            self._cmd_pub.publish(self.twist)
            # rospy.spin()

def main():
    rospy.init_node('laser_avoid')
    try:
        obstacle = Obstacle()
    except rospy.ROSInterruptException:
        pass

if __name__ == '__main__':
    main()
